#include <pthread.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <signal.h>

#include <arpa/inet.h>
#include <netdb.h>
#include <netinet/in.h>
#include <sys/types.h>
#include <sys/socket.h>

#include <termios.h>//for keyboard and serial port

#include <fcntl.h>
#include <linux/i2c-dev.h>

#define I2C_SLAVE	0x0703	/* Change slave address			*/


int simu=0;

static char filename3[20];
static int i2cbus=0,file;

static unsigned char bufset[10];
static unsigned char bufget[3];
static unsigned char bufad[1];

#define NB_THREADS	6

static int initI2c();

void gestionnaire (int numero);

static int compteur = 0;
pthread_t thread [NB_THREADS];
int       i;
int       ret;

int dog=0;
pthread_mutex_t	mutex_dog = PTHREAD_MUTEX_INITIALIZER;

#define MAXPENDING 5    /* Maximum outstanding connection requests */
static int servSock;                    /* Socket descriptor for server */
static int clntSock;                    /* Socket descriptor for client */
static struct sockaddr_in echoServAddr; /* Local address */
static struct sockaddr_in echoClntAddr; /* Client address */
static unsigned short echoServPort=15000;     /* Server port */
static unsigned int clntLen;            /* Length of client address data structure */
static int recvMsgSize=0; 
static unsigned char buffso_rcv[32]; 
static unsigned char buffso_send[7];

int first=1;
static int connected=0;
int debug=0;
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
typedef struct 
	{
	int SpeedFront;
	int SpeedRear;
	int IR;
	int IR2;
	int robot_voltage;
	int odometry;
	int robot_current;
	int robot_version;
	}SensorData;

int Data_Of_Thread_In = 1;            // Data of Thread 1
char buffso_rcvtcp[32]; 
char buffso_sendtcp[7];

//Trooper
int servermode = 0;
int watchdog = 0;
int cycles = 0;
unsigned char speed1 = 0;
unsigned char speed2 = 0;

char buffso_rcvIN[100]; 
char buffso_rcvOUT[100]; 
char buffso_sendIN[100];
char buffso_sendOUT[100];
char buffer_address[sizeof(struct sockaddr)] = "0.0.0.0";
char buffer_localip[sizeof(struct sockaddr)] = "0.0.0.0";

int tcpon=0;
SensorData dataL;
SensorData dataR;
struct termios oldoptions,newoptions;
void raw_mode(int fd);

int SetupI2CCommPort(int comport);

void * Thread_TCP(void * lpParam);
void * Thread_TCP_Trooper_In(void * lpParam );
void * Thread_TCP_Trooper_Out(void * lpParam );
void * Thread_TCP_Trooper_I2C(void * lpParam );
void * Thread_TCP_4G_I2C(void * lpParam );
void * Thread_Dog(void * lpParam );

int GetFirmware(int hUSB);
SensorData GetI2cMotor(int hUSB, char add);
int SetI2cMotor(int hUSB, char add,char speed);
int GetADC(int hUSB,char add);
int StopMotor(int hUSB);

SensorData GetI2cMotor4G(int filei2c, char add);
int SetI2cMotor4G(int filei2c, char add,char speed);
int getAD4G(int filei2c, char add);
int StopMotor4G(int hUSB);

//#define debug_msg

#define I2CD_CMD	0x53		// direct I2C control command
#define I2C_CMD		0x55		// registered I2C control command
#define USBI2C_CMD	0x5a		// Module command 

enum cmds { nop=0, VERSION, NEW_ADDRESS, BATTERY,
			SCAN1, SCAN2, SCAN3, SCAN4, SCAN6, SCAN8, SCAN12, SCAN16,
			SETPINS=0x10, GETPINS, GETAD,
};

enum state { Done=0, Off, On };

int hUSB=0;
int fd=0;

pthread_mutex_t	mutex_send = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t	mutex_rcv = PTHREAD_MUTEX_INITIALIZER;

////Threads/////////////////////////////////////////////
int socket_in;
int socket_out;
int socket_tcp;
struct sockaddr_in myaddr_in;
struct sockaddr_in myaddr_out;
struct sockaddr_in myaddr_tcp;
struct sockaddr_in client_in;
struct sockaddr_in client_out;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
